// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcPolynomial.h"
#include "MgcTCBSpline3.h"

//----------------------------------------------------------------------------
MgcTCBSpline3::MgcTCBSpline3 (int iSegments, MgcReal* afTime,
    MgcVector3* akPoint, MgcReal* afTension, MgcReal* afContinuity,
    MgcReal* afBias)
    :
    MgcMultipleCurve3(iSegments,afTime)
{
    // TO DO.  Add 'boundary type' just as in natural splines.
    assert( m_iSegments >= 3 );

    // all four of these arrays have m_iSegments+1 elements
    m_akPoint = akPoint;
    m_afTension = afTension;
    m_afContinuity = afContinuity;
    m_afBias = afBias;

    m_akA = new MgcVector3[m_iSegments];
    m_akB = new MgcVector3[m_iSegments];
    m_akC = new MgcVector3[m_iSegments];
    m_akD = new MgcVector3[m_iSegments];

    // For now, treat the first point as if it occurred twice.
    ComputePoly(0,0,1,2);

    for (int i = 1; i < m_iSegments-1; i++)
        ComputePoly(i-1,i,i+1,i+2);

    // For now, treat the last point as if it occurred twice.
    ComputePoly(m_iSegments-2,m_iSegments-1,m_iSegments,m_iSegments);

}
//----------------------------------------------------------------------------
MgcTCBSpline3::~MgcTCBSpline3 ()
{
    delete[] m_akPoint;
    delete[] m_afTension;
    delete[] m_afContinuity;
    delete[] m_afBias;
    delete[] m_akA;
    delete[] m_akB;
    delete[] m_akC;
    delete[] m_akD;
}
//----------------------------------------------------------------------------
void MgcTCBSpline3::ComputePoly (int i0, int i1, int i2, int i3)
{
    MgcVector3 kDiff = m_akPoint[i2] - m_akPoint[i1];
    MgcReal fDt = m_afTime[i2] - m_afTime[i1];

    // build multipliers at P1
    MgcReal fOmt0 = 1.0 - m_afTension[i1];
    MgcReal fOmc0 = 1.0 - m_afContinuity[i1];
    MgcReal fOpc0 = 1.0 + m_afContinuity[i1];
    MgcReal fOmb0 = 1.0 - m_afBias[i1];
    MgcReal fOpb0 = 1.0 + m_afBias[i1];
    MgcReal fAdj0 = 2.0*fDt/(m_afTime[i2]-m_afTime[i0]);
    MgcReal fOut0 = 0.5*fAdj0*fOmt0*fOpc0*fOpb0;
    MgcReal fOut1 = 0.5*fAdj0*fOmt0*fOmc0*fOmb0;

    // build outgoing tangent at P1
    MgcVector3 kTOut = fOut1*kDiff + fOut0*(m_akPoint[i1] - m_akPoint[i0]);

    // build multipliers at point P2
    MgcReal fOmt1 = 1.0 - m_afTension[i2];
    MgcReal fOmc1 = 1.0 - m_afContinuity[i2];
    MgcReal fOpc1 = 1.0 + m_afContinuity[i2];
    MgcReal fOmb1 = 1.0 - m_afBias[i2];
    MgcReal fOpb1 = 1.0 + m_afBias[i2];
    MgcReal fAdj1 = 2.0*fDt/(m_afTime[i3] - m_afTime[i1]);
    MgcReal fIn0 = 0.5*fAdj1*fOmt1*fOmc1*fOpb1;
    MgcReal fIn1 = 0.5*fAdj1*fOmt1*fOpc1*fOmb1;

    // build incoming tangent at P2
    MgcVector3 kTIn = fIn1*(m_akPoint[i3] - m_akPoint[i2]) + fIn0*kDiff;

    m_akA[i1] = m_akPoint[i1];
    m_akB[i1] = kTOut;
    m_akC[i1] = 3.0*kDiff - 2.0*kTOut - kTIn;
    m_akD[i1] = -2.0*kDiff + kTOut + kTIn;
}
//----------------------------------------------------------------------------
MgcVector3 MgcTCBSpline3::GetPosition (MgcReal fTime) const
{
    int iKey;
    MgcReal fDt;
    GetKeyInfo(fTime,iKey,fDt);

    fDt /= (m_afTime[iKey+1] - m_afTime[iKey]);

    MgcVector3 kResult = m_akA[iKey] + fDt*(m_akB[iKey] + fDt*(
        m_akC[iKey] + fDt*m_akD[iKey]));

    return kResult;
}
//----------------------------------------------------------------------------
MgcVector3 MgcTCBSpline3::GetFirstDerivative (MgcReal fTime) const
{
    int iKey;
    MgcReal fDt;
    GetKeyInfo(fTime,iKey,fDt);

    fDt /= (m_afTime[iKey+1] - m_afTime[iKey]);

    MgcVector3 kResult = m_akB[iKey] + fDt*(2.0*m_akC[iKey] + 3.0*fDt*
        m_akD[iKey]);

    return kResult;
}
//----------------------------------------------------------------------------
MgcVector3 MgcTCBSpline3::GetSecondDerivative (MgcReal fTime) const
{
    int iKey;
    MgcReal fDt;
    GetKeyInfo(fTime,iKey,fDt);

    fDt /= (m_afTime[iKey+1] - m_afTime[iKey]);

    MgcVector3 kResult = 2.0*m_akC[iKey] + 6.0*fDt*m_akD[iKey];

    return kResult;
}
//----------------------------------------------------------------------------
MgcVector3 MgcTCBSpline3::GetThirdDerivative (MgcReal fTime) const
{
    int iKey;
    MgcReal fDt;
    GetKeyInfo(fTime,iKey,fDt);

    fDt /= (m_afTime[iKey+1] - m_afTime[iKey]);

    MgcVector3 kResult = 6.0*m_akD[iKey];

    return kResult;
}
//----------------------------------------------------------------------------
MgcReal MgcTCBSpline3::GetSpeed (int iKey, MgcReal fTime) const
{
    MgcVector3 kVelocity = m_akB[iKey] + fTime*(2.0*m_akC[iKey] +
        3.0*fTime*m_akD[iKey]);
    return kVelocity.Length();
}
//----------------------------------------------------------------------------
MgcReal MgcTCBSpline3::GetLength (int iKey, MgcReal fT0, MgcReal fT1) const
{
    class ThisPlusKey
    {
    public:
        ThisPlusKey (const MgcTCBSpline3* pkThis, int iKey)
            : m_pkThis(pkThis), m_iKey(iKey) { /**/ }

        const MgcTCBSpline3* m_pkThis;
        int m_iKey;
    };

    ThisPlusKey kData(this,iKey);

    return MgcIntegrate::RombergIntegral(fT0,fT1,GetSpeedWithData,
        (void*)&kData);
}
//----------------------------------------------------------------------------
MgcReal MgcTCBSpline3::GetVariation (int iKey, MgcReal fT0, MgcReal fT1,
    const MgcVector3& rkA, const MgcVector3& rkB) const
{
    MgcPolynomial kXPoly(3);
    kXPoly[0] = m_akA[iKey].x;
    kXPoly[1] = m_akB[iKey].x;
    kXPoly[2] = m_akC[iKey].x;
    kXPoly[3] = m_akD[iKey].x;

    MgcPolynomial kYPoly(3);
    kYPoly[0] = m_akA[iKey].y;
    kYPoly[1] = m_akB[iKey].y;
    kYPoly[2] = m_akC[iKey].y;
    kYPoly[3] = m_akD[iKey].y;

    MgcPolynomial kZPoly(3);
    kZPoly[0] = m_akA[iKey].z;
    kZPoly[1] = m_akB[iKey].z;
    kZPoly[2] = m_akC[iKey].z;
    kZPoly[3] = m_akD[iKey].z;

    // construct line segment A + t*B
    MgcPolynomial kLx(1), kLy(1), kLz(1);
    kLx[0] = rkA.x;
    kLx[1] = rkB.x;
    kLy[0] = rkA.y;
    kLy[1] = rkB.y;
    kLz[0] = rkA.z;
    kLz[1] = rkB.z;

    // compute |X(t) - L(t)|^2
    MgcPolynomial kDx = kXPoly - kLx;
    MgcPolynomial kDy = kYPoly - kLy;
    MgcPolynomial kDz = kZPoly - kLz;
    MgcPolynomial kNormSqr = kDx*kDx + kDy*kDy + kDz*kDz;

    // compute indefinite integral of |X(t)-L(t)|^2
    MgcPolynomial kIntegral(kNormSqr.GetDegree()+1);
    kIntegral[0] = 0.0;
    for (int i = 1; i <= kIntegral.GetDegree(); i++)
        kIntegral[i] = kNormSqr[i-1]/i;

    // compute definite Integral(t0,t1,|X(t)-L(t)|^2)
    MgcReal fResult = kIntegral(fT1) - kIntegral(fT0);
    return fResult;
}
//----------------------------------------------------------------------------
